/*
	Cloud data loading, storing and converting
*/
#pragma once 
#include "stdafx.h"
#include <pcl/io/openni_grabber.h>
#include <boost/filesystem.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/io/oni_grabber.h>
#include "LitemViewer.h"
#include "CloudProcessor.h"
#include "ChangeDetector.h"


using namespace boost::filesystem;

class SimpleOpenNIStreamer {
protected:
	string view_name;
	boost::shared_ptr<pcl::visualization::CloudViewer> viewer;
	virtual void cloud_cb_ (const LITEM_CLOUD::ConstPtr &cloud) {
		if (!viewer->wasStopped()) {
			viewer->showCloud (cloud);
			boost::this_thread::sleep (boost::posix_time::milliseconds(100));
		}
	}
	void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,void* nothing) { 
		if (event.getKeySym() == "space" && event.keyDown()) {
			cout << "space pressed" << endl; 
		}
    } 
public:
	SimpleOpenNIStreamer(string name) : view_name(name){
		viewer = boost::shared_ptr<pcl::visualization::CloudViewer>(new pcl::visualization::CloudViewer(name));
		viewer->registerKeyboardCallback(&SimpleOpenNIStreamer::keyboardEventOccurred, *this, (void*)&viewer); 
	}
  
	virtual void run () {
		// create a new grabber for OpenNI devices
		pcl::ONIGrabber* interface = new pcl::ONIGrabber(view_name, false, true);
		//pcl::Grabber* interface = new pcl::OpenNIGrabber();

		// make callback function from member function
		boost::function<void (const LITEM_CLOUD::ConstPtr&)> f = boost::bind(&SimpleOpenNIStreamer::cloud_cb_, this, _1);

		// connect callback function for desired signal. In this case its a point cloud with color values
		boost::signals2::connection c = interface->registerCallback (f);

		// start receiving point clouds
		interface->start ();

		// wait until user quits program with Ctrl-C, but no busy-waiting -> sleep (1);
		while (true)
		  boost::this_thread::sleep (boost::posix_time::seconds (1));

		// stop the grabber
		interface->stop ();
		
	}
};

class CloudLoader {
private:
	int n;
	vector<string> clouds;
public:
	CloudLoader() {}
	int init(string str) {
		if(!exists(str)) {
			cout << "No "+str+" exists";
			return -1;
		}
		//fill in the names of point clouds in folder
		path dir_root(str);
		directory_iterator end_itr; // default construction yields past-the-end
		for ( directory_iterator itr( dir_root ); itr != end_itr; ++itr ) {
			if(itr->path().has_filename()) {
				string cloud = canonical(itr->path()).string();
				string format = cloud.substr(cloud.size()-2, 3);
				if(format.compare("pcd")) {
					clouds.push_back(cloud);
				}
			}
		} 
		n = 0;
		return 0;
	}
	int nextCloud(LITEM_CLOUD::Ptr cloud) {
		if(n<clouds.size()) {
			pcl::io::loadPCDFile(clouds[n++], *cloud);
			return 0;
		}
		return -1;
	}
};

//Stream cloud data from .oni files into point cloud
class LitemGrabber {
private:
	//input .oni file name
	string oni_name;

	CloudProcessor * processing;
	ChangeDetector * changeDetector;
	
	//total number of processed frames
	int n;
	
	void cloud_cb_ (const LITEM_CLOUD::ConstPtr &callback_cloud, LITEM_CLOUD *cloud_object, bool* new_cloud_available_flag) {
		pcl::copyPointCloud<LITEM_POINT, LITEM_POINT>(*callback_cloud, *cloud_object);
		*new_cloud_available_flag = true;
		cout << n << " frames " << endl;
		n++;
	}

	// PCL viewer //
	pcl::visualization::PCLVisualizer viewer;

public:
	LitemGrabber(string oni_name) : oni_name(oni_name), n(0) {
	}
	~LitemGrabber() {
		delete processing;
		delete changeDetector;
	}
	void run() {
		LitemViewer viewer;
		ChangeDetector changeDetector;
		//initialize processors, specify parameters
		//viewerInput = new LitemViewer("Oni grabber");
		//viewerOutput = new LitemViewer("Processing");
		processing = new CloudProcessor();
		//changeDetector = new ChangeDetector();

		//register processors
		//processors are invoked in the order they were added to the queue
		//processing->registerProcessor(changeDetector);

		//.oni reading 
		LITEM_CLOUD cloud_obj;
		LITEM_CLOUD::Ptr cloud (new LITEM_CLOUD);
		bool new_cloud_available_flag = false;

		// create a new grabber for OpenNI devices
		pcl::ONIGrabber* interface = new pcl::ONIGrabber(oni_name, false, true);
		
		//register callback
		boost::function<void (const LITEM_CLOUD::ConstPtr&)> f = boost::bind (&LitemGrabber::cloud_cb_,this, _1, &cloud_obj, &new_cloud_available_flag);
	    interface->registerCallback (f);
		
		// start receiving point clouds
		interface->start ();

		LITEM_CLOUD::Ptr outCloud(new LITEM_CLOUD);
		// Main loop:
		while (!viewer.stopped()) {
			boost::this_thread::sleep (boost::posix_time::milliseconds (40));
			if (new_cloud_available_flag && !viewer.isAsleep())  {
				// Make the "cloud" pointer point to the new cloud:
				pcl::copyPointCloud<LITEM_POINT, LITEM_POINT>(cloud_obj, *cloud);
				new_cloud_available_flag = false;
				viewer.view(cloud);
				changeDetector.process(cloud, outCloud);
				viewer.view(outCloud, "processed");
			}
			//interface->start();
		}
		// stop the grabber
		interface->stop ();
	}
};